clear,clc
%% 参数设置（来自表B4，调整部分参数增强稳定性）
Rr = 0.0013;       % 转子绕组电阻
Xs = 2.9;          % 定子绕组漏抗
Xr = 2.9;          % 转子绕组漏抗
Xm = 2.6;          % 磁感电抗
Tj = 3.4;          % 惯性时间常数
D = 20;            % 增大阻尼系数以抑制振荡（原值5→20）
C = 13.29;         % 中间电容容值
Xl3 = 5;           % 滤波电抗值
omega_s = 1.0;     % 同步角速度

% 计算派生参数
Xn = Xm + Xr;  % 转子侧总阻抗             
Xp = Xm + Xs - (Xm^2)/Xn;  % 定子侧等效阻抗
Ts = 0.05;                 % 减小时间步长（原0.01→0.005）采样时间（降低步长提升精度）
N = 1000;              % 总仿真步数    

%% 状态变量定义与初始化（设置合理稳态初值）
n=6;
x = zeros(6, N);    
X = zeros(6, N);
X_UKF = zeros(6, N);
XMinus=zeros(6, N);
x_cankao=zeros(6, N);
% 稳态初始值（通过稳态方程计算）
x(:,1) = [0.8; 0.6; 0.01; 1.0; 0.05; 0.03];
x_cankao(:, 1)=[0.8; 0.6; 0.01; 1.0; 0.05; 0.03];
X(:,1) = x(:,1) + 0.2*randn(6,1);  % uktm算法初值，增大初始噪声
X_UKF(:,1) = X(:,1);%单UKF算法初值

%% 输入变量定义（限制幅值并平滑变化）
t = (0:N-1)*Ts;
% d轴转子电压（添加小幅波动）
U_dr = 0.2 + 0.02*sin(2*pi*0.2*t);  % 减小输入波动幅度
% q轴转子电压（余弦波动）
U_qr = 0.1 + 0.01*cos(2*pi*0.2*t);% 机械转矩（低频变化）
T_m = 0.8 + 0.05*sin(2*pi*0.1*t);   % 机械转矩平稳变化
I_dl = 0.5 + 0.01*randn(1,N);       % 减小负载噪声% 负载电流（含小噪声）

%% 状态方程离散化（RK4方法，增加U_dc保护）
for k = 1:N-1
    % 直流电压保护（防止过零点）
    x(4,k) = max(x(4,k), 0.1);  
    
    % RK4积分 % 四阶龙格库塔积分
    k1 = state_model(x(:,k), U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    k2 = state_model(x(:,k) + Ts/2*k1, U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    k3 = state_model(x(:,k) + Ts/2*k2, U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    k4 = state_model(x(:,k) + Ts*k3,   U_dr(k), U_qr(k), T_m(k), I_dl(k), Ts);
    % x(:,k+1) = x(:,k) + Ts/6*(k1 + 2*k2 + 2*k3 + k4)+ 0.009*randn(6,1);
    % 大幅增加过程噪声
    x(:,k+1) = x(:,k) + Ts/6*(k1 + 2*k2 + 2*k3 + k4) + 0.5*randn(6,1); % 噪声从0.009→0.5
end

%% 观测方程（添加量测噪声）（添加高斯白噪声）
m=6;
% z = zeros(6, N);
% for k = 1:N 
%     % 真实量测值 + 量测噪声（SNR≈60dB）
%     % z(:,k) = meas_model(x(:,k)) + 0.009*randn(6,1);% 添加小量噪声，原始0.001，增加为0.009
%      % 真实量测值 + 剧烈量测噪声
%     z(:,k) = meas_model(x(:,k)) + 0.5*randn(6,1); % 噪声从0.009→0.5
% end
%% 观测方程（添加脉冲噪声 + 高斯噪声）
z = zeros(6, N);
impulse_prob = 0.1;  % 脉冲发生概率10%
impulse_mag = 3.0;   % 脉冲幅度（原信号幅值约0.5-1.5）
for k = 1:N 
    z_k = meas_model(x(:,k)) + 0.5*randn(6,1);
    % 随机添加脉冲噪声
    if rand() < impulse_prob
        impulse = impulse_mag * (2*(rand(6,1)>0.5)-1); % 随机正负脉冲
        z(:,k) = z_k + impulse+ 0.5*randn(6,1);
    else
        z(:,k) = z_k+ 0.5*randn(6,1);
    end
end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%% UKF-LSTM融合算法核心部分 %%%%%%%%%%%%%%%%%%%%
%% UKF参数设置（错误配置噪声协方差）
Q = diag([1e-4, 1e-4, 1e-6, 1e-6, 1e-6, 1e-6]);  % 低估过程噪声
R =  diag([1e-4, 1e-4, 1e-6, 1e-6, 1e-6, 1e-6]); % 低估量测噪声
alpha = 0.1;  % 缩小Sigma点分布范围
Pplus = eye(n)*0.1;                               % 初始协方差
% alpha =1;     % 调节Sigma点分布,增大扩大Sigma点分布,1的效果还可以，越大曲线拟合越快
beta = 2;         % 优化高斯分布假设，原值是2
kappa = 0;        % 通常设为0
lambda = alpha^2 * (n + kappa) - n;
                            

%% LSTM参数
numFeatures=10; % 输入特征维度（状态+输入+参考）
numResponses=6;% 输出响应维度（状态维度）
numHiddenUnits=300;% 隐藏层神经元数量
layers = [ 
    sequenceInputLayer(numFeatures) % 输入层
    batchNormalizationLayer        % 批标准化
    lstmLayer(128)                 % 128单元LSTM层
    dropoutLayer(0.3)              % 30%丢弃率防过拟合
    lstmLayer(64)                  % 64单元LSTM层
    fullyConnectedLayer(numResponses) % 全连接输出层
    regressionLayer];              % 回归层

MaxEpochs=300;
InitialLearnRate=0.01;


%% 误差指标初始化
RMSE = zeros(6,N);  % 均方根误差
MSE = zeros(6,N);   % 均方误差
MAE = zeros(6,N);   % 平均绝对误差



%% UKF-LSTM主循环
for k = 2:N
    % ========== UKF预测阶段 ==========
    % 生成Sigma点集
    [sigma_points, weights] = ut_weights(X(:,k-1), Pplus, lambda, alpha, beta);
    
    % 并行计算Sigma点预测（RK4积分）
    Xsig_pred = zeros(n, 2*n+1);
    for i = 1:2*n+1
        x_i = sigma_points(:,i);
        % 四阶龙格库塔积分
        k1 = state_model(x_i, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        k2 = state_model(x_i + Ts/2*k1, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        k3 = state_model(x_i + Ts/2*k2, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        k4 = state_model(x_i + Ts*k3, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        Xsig_pred(:,i) = x_i + Ts/6*(k1 + 2*k2 + 2*k3 + k4);
    end
    % 计算预测均值与协方差
    Xminus_UKF = Xsig_pred * weights.m;
    Pminus = (Xsig_pred - Xminus_UKF) * diag(weights.c) * (Xsig_pred - Xminus_UKF)' + Q;
     x_cankao(:,k-1)=X(:,k-1)-0.3*randn(6,1);
    
% ========== LSTM预测阶段 ==========
    % 构造训练数据集（滚动时间窗）
    if k<=101
    data = [X(:,1:k-1);           % 历史状态
            U_dr(1:k-1);          % 控制输入
            U_qr(1:k-1);
            T_m(1:k-1);
            I_dl(1:k-1);
            
           x_cankao(:,1:k-1)];         % 真实状态参考
    else
        data = [X(:,k-100:k-1);           % 历史状态
            U_dr(k-100:k-1);          % 控制输入
            U_qr(k-100:k-1);
            T_m(k-100:k-1);
            I_dl(k-100:k-1);
            
            x_cankao(:,k-100:k-1)];   
    end
    % 数据标准化处理
    mu = mean(data(:));
    sig = std(data(:));
    XTrain = (data(1:10,:) - mu)/sig;
    YTrain = (data(11:16,:) - mu)/sig; % 目标为真实状态
    
    % LSTM网络训练配置
    options = trainingOptions('adam',...
        'MaxEpochs',MaxEpochs,...
        'MiniBatchSize',128,...
        'InitialLearnRate',InitialLearnRate,...
        'Plots','none');
    
    % 网络训练与预测
    %% 在线阶段加载预训练网络



    net = trainNetwork(XTrain,YTrain,layers,options);
    [net, YPred] = predictAndUpdateState(net, XTrain);
    Xminus_LSTM = sig*YPred(:,end) + mu; % 提取最新预测

    % ========== 融合策略 ==========
    % 计算各方法预测误差
    error_UKF = norm(Xminus_UKF - x_cankao(:,k));
    error_LSTM = norm(Xminus_LSTM - x_cankao(:,k));
  
    
    if (error_UKF==error_LSTM)
        w_UKF=0.5;
        w_LSTM=0.5;
    else
        total_error = sum(error_UKF) + sum(error_LSTM);
        w_UKF = sum(error_LSTM) / total_error;
        w_LSTM = 1 - w_UKF;
    end
    
    % 加权融合预测值
    Xminus_fused = w_UKF*Xminus_UKF + w_LSTM*Xminus_LSTM;

    % ========== UKF更新阶段 ==========
    % 量测预测
    Zsig = zeros(m, 2*n+1);
    for i = 1:2*n+1
        Zsig(:,i) = meas_model(Xsig_pred(:,i));
    end
    Zminus = Zsig * weights.m;
    
    % 卡尔曼增益计算
    Pxz = (Xsig_pred - Xminus_fused) * diag(weights.c) * (Zsig - Zminus)';
    Pzz = (Zsig - Zminus) * diag(weights.c) * (Zsig - Zminus)' + R;
    K = Pxz / Pzz;
    
    % 状态更新
    X(:,k) = Xminus_fused + K * (z(:,k) - Zminus);
    Pplus = Pminus - K * Pzz * K';

    % ========== 误差统计 ==========
    for i = 1:6
        ae = abs(X(i,k) - x(i,k));
        RMSE(i,k) = sqrt(mean(ae^2));
        MSE(i,k) = mean(ae^2);
        MAE(i,k) = mean(ae);
    end
end
%% 其余部分保持不变（UKF单独实现及绘图）
% % UKF循环过程噪声和量测噪声协方差
% Q = diag([1e-2, 1e-2, 1e-4, 1e-4, 1e-4, 1e-4]);  % 根据论文调整
% R =  diag([1e-2, 1e-2, 1e-4, 1e-4, 1e-4, 1e-4]);               % 量测噪声
%% UKF参数设置（错误配置噪声协方差）
Q = diag([1e-4, 1e-4, 1e-6, 1e-6, 1e-6, 1e-6]);  % 低估过程噪声
R =  diag([1e-4, 1e-4, 1e-6, 1e-6, 1e-6, 1e-6]); % 低估量测噪声
alpha = 0.1;  % 缩小Sigma点分布范围
Pplus = eye(n)*0.1;                               % 初始协方差
% alpha =1;     % 调节Sigma点分布,增大扩大Sigma点分布,1的效果还可以，越大曲线拟合越快
beta = 2;         % 优化高斯分布假设，原值是2
kappa = 0;        % 通常设为0
lambda = alpha^2 * (n + kappa) - n;
%% 误差定义
RMSE1=zeros(6,N);
MSE1=zeros(6,N);
MAE1 =zeros(6,N);
%% UKF主循环
for k = 2:N
    % Sigma点生成
    [sigma_points, weights] = ut_weights(X_UKF(:,k-1), Pplus, lambda, alpha, beta);
    
    % 状态预测（RK4积分）
    Xsig_pred = zeros(n, 2*n+1);
    for i = 1:2*n+1
        x_i = sigma_points(:,i);
        k1 = state_model(x_i, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        k2 = state_model(x_i + Ts/2*k1, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        k3 = state_model(x_i + Ts/2*k2, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        k4 = state_model(x_i + Ts*k3, U_dr(k-1), U_qr(k-1), T_m(k-1), I_dl(k-1), Ts);
        Xsig_pred(:,i) = x_i + Ts/6*(k1 + 2*k2 + 2*k3 + k4);
    end
    Xminus = Xsig_pred * weights.m;
    Pminus = (Xsig_pred - Xminus) * diag(weights.c) * (Xsig_pred - Xminus)' + Q;
    
    % 量测预测
    Zsig = zeros(m, 2*n+1);
    for i = 1:2*n+1
        Zsig(:,i) = meas_model(Xsig_pred(:,i));
    end
    Zminus = Zsig * weights.m;
    
  % 卡尔曼增益
    Pxz = (Xsig_pred - Xminus) * diag(weights.c) * (Zsig - Zminus)';
    Pzz = (Zsig - Zminus) * diag(weights.c) * (Zsig - Zminus)' + R;
    K = Pxz / Pzz;
    
    % 状态更新
    X_UKF(:,k) = Xminus + K * (z(:,k) - Zminus);
    Pplus = Pminus - K * Pzz * K';

      %% 评价
    ae11= abs(X_UKF(1,k) - x(1,k));
    ae12= abs(X_UKF(2,k) - x(2,k));
    ae13= abs(X_UKF(3,k) - x(3,k));
    ae14= abs(X_UKF(4,k) - x(4,k));
    ae15= abs(X_UKF(5,k) - x(5,k));
    ae16= abs(X_UKF(6,k) - x(6,k));
    RMSE1(1,k) = (mean(ae11.^2)).^0.5;
    RMSE1(2,k) = (mean(ae12.^2)).^0.5;
    RMSE1(3,k) = (mean(ae13.^2)).^0.5;
    RMSE1(4,k) = (mean(ae14.^2)).^0.5;
    RMSE1(5,k) = (mean(ae15.^2)).^0.5;
    RMSE1(6,k) = (mean(ae16.^2)).^0.5;
 
    MSE1(1,k) = mean(ae11.^2);
    MSE1(2,k) = mean(ae12.^2);
    MSE1(3,k) = mean(ae13.^2);
    MSE1(4,k) = mean(ae14.^2);
    MSE1(5,k) = mean(ae15.^2);
    MSE1(6,k) = mean(ae16.^2); 

    MAE1(1,k)  = mean(ae11);
    MAE1(2,k)  = mean(ae12);
    MAE1(3,k)  = mean(ae13);
    MAE1(4,k)  = mean(ae14);
    MAE1(5,k)  = mean(ae15);
    MAE1(6,k)  = mean(ae16);
 
    disp('预测结果评价指标：')
     disp(['RMSE = ', num2str(RMSE1(1,k))])
    disp(['MSE  = ', num2str(MSE1 (2,k))])
    disp(['MAE  = ', num2str(MAE1(3,k))])
end  
% %% 状态估计结果对比图
% state_names = {'E_d', 'E_q', '滑差s', 'U_{dc}', 'I_{rd3}', 'I_{rq3}'};
% figure('Name','状态估计对比','WindowState','maximized')
% for i = 1:6
%     subplot(2,3,i)
%     plot(t, x(i,:), 'b', 'LineWidth', 1.5) 
%     hold on
%     plot(t, X_UKF(i,:), 'r--', 'LineWidth', 1.2)
%     plot(t, z(i,:), 'Color',[0.2 0.6 0.2], 'LineStyle',':')
%     title([state_names{i} ' 估计对比'],'FontSize',10)
%     xlabel('时间/s','FontSize',9)
%     ylabel(state_names{i},'FontSize',9)
%     legend('真实状态','UKF估计','量测值','Location','best')
%     grid on
% end
% 
% %% 估计误差分析图（RMSE）
% figure('Name','误差分析','WindowState','maximized') 
% error_names = {'E_d RMSE','E_q RMSE','滑差s RMSE','U_{dc} RMSE','I_{rd3} RMSE','I_{rq3} RMSE'};
% for i = 1:6
%     subplot(2,3,i)
%     semilogy(t, RMSE1(i,:), 'Color',[0.8 0.2 0.6], 'LineWidth',1.3) % 对数坐标
%     title(error_names{i},'FontSize',10)
%     xlabel('时间/s','FontSize',9)
%     ylabel('误差值(pu)','FontSize',9)
%     grid on
%     ylim([1e-4 1e-1])
% end
%% 绘图（添加融合结果对比）
figure;
subplot(2,3,1);
plot(t, x(1,:), 'r', t, X(1,:), 'b', t, X_UKF(1,:), 'g');
title('E_d''估计');
legend('真实值', 'UKF-LSTM融合估计','UKF估计');
subplot(2,3,2);
plot(t, x(2,:), 'r', t, X(2,:), 'b--', t, X_UKF(2,:), 'g');
title('E_q''估计');
legend('真实值', 'UKF-LSTM估计','UKF估计');

subplot(2,3,3);
plot(t, x(3,:), 'r', t, X(3,:), 'b--', t, X_UKF(3,:), 'g');
title('转差率s''估计');
legend('真实值', 'UKF-LSTM估计','UKF估计');

subplot(2,3,4);
plot(t, x(4,:), 'r', t, X(4,:), 'b--', t, X_UKF(4,:), 'g');
title('直流电压U_{dc}''估计');
legend('真实值', 'UKF-LSTM估计','UKF估计');

subplot(2,3,5);
plot(t, x(5,:), 'r', t, X(5,:), 'b--', t, X_UKF(5,:), 'g');
title('转子d轴电流I_{rd3}''估计');
legend('真实值', 'UKF-LSTM估计','UKF估计');

subplot(2,3,6);
plot(t, x(6,:), 'r', t, X(6,:), 'b--', t, X_UKF(6,:), 'g');
title('转子q轴电流I_{rq3}''估计');
legend('真实值', 'UKF-LSTM估计','UKF估计');

%% 函数定义
function [sigma_points, weights] = ut_weights(x, P, lambda, alpha, beta)
    n = length(x);
    sigma_points = zeros(n, 2*n+1);
    weights = struct('m', zeros(2*n+1,1), 'c', zeros(2*n+1,1));
    
    gamma = sqrt(n + lambda);
    sigma_points(:,1) = x;
    weights.m(1) = lambda / (n + lambda);
    weights.c(1) = weights.m(1) + (1 - alpha^2 + beta);
    
    [U, S, ~] = svd(P);
    S_sqrt = U * sqrt(S) * U';
    for i = 1:n
        sigma_points(:,i+1) = x + gamma * S_sqrt(:,i);
        sigma_points(:,n+i+1) = x - gamma * S_sqrt(:,i);
        weights.m(i+1) = 1 / (2*(n + lambda));
        weights.m(n+i+1) = weights.m(i+1);
        weights.c(i+1) = weights.m(i+1);
        weights.c(n+i+1) = weights.m(i+1);
    end
end

%% 状态方程修正（关键模型调整）
function dx = state_model(x, U_dr, U_qr, T_m, I_dl, Ts)
    Rr = 0.0013;    
    Xm = 2.6;      
    Xn = Xm + 2.9;  
    Tj = 3.4;      
    D = 20;         % 同步修改阻尼系数
    C = 13.29;     
    Xl3 = 5;       
    omega_s = 1.0; 

    E_d = x(1);
    E_q = x(2);
    s = x(3);
    U_dc = max(x(4), 0.1);  % 确保U_dc>0
    I_rd3 = x(5);
    I_rq3 = x(6);
    
    omega_r = omega_s * (1 - s);
    
    % 修正电磁转矩计算（根据论文式(1)）
    Tc = (E_d*I_rd3 + E_q*I_rq3);  % 正确电磁转矩表达式
    
    % 状态方程（修正符号与系数）
    dE_d = omega_s * (-Rr/Xn*E_d + s*E_q + (Rr*Xm/Xn)*I_rq3 - (Xm/Xn)*U_qr);
    dE_q = omega_s * (-Rr/Xn*E_q - s*E_d - (Rr*Xm/Xn)*I_rd3 + (Xm/Xn)*U_dr);

    % 修正转差率方程（增加非线性项补偿）
    % ds = (T_m - Tc - D*(s - 0.01) - 0.1*s^3) / Tj; %修正后
    ds = (T_m - Tc - D*(s - 0.01)) / Tj;  % 原方程，明确阻尼项
    

    % 直流电压方程增加漏电流补偿
    % dU_dc = (I_rd3^2 + I_rq3^2 - I_dl - 0.01*U_dc) / (C * U_dc);
    % 修正直流电压方程（根据论文式(2)）
    dU_dc = (I_rd3^2 + I_rq3^2 - I_dl) / (C * U_dc);%原方程
    
    % 修正网侧变流器方程（根据论文式(3)）
    dI_rd3 = (U_dr - U_dc + Xl3*omega_r*I_rq3) / Xl3;
    dI_rq3 = (U_qr - U_dc - Xl3*omega_r*I_rd3) / Xl3;
    
    dx = [dE_d; dE_q; ds; dU_dc; dI_rd3; dI_rq3];
end

%% 量测方程（保持与论文一致）
function z = meas_model(x)
    E_d = x(1);
    E_q = x(2);
    s = x(3);
    U_dc = max(x(4), 0.1);  % 确保U_dc>0
    I_rd3 = x(5);
    I_rq3 = x(6);
    
    z = [E_d ;
    E_q ;
    s ;
    U_dc ;  % 确保U_dc>0
    I_rd3 ;
    I_rq3 ;  
    ];
end